function [imagen,X,Z]=BeamFormingActivo(pulso,eco,pitch,vel_prop,retardo,fs,ResX,ResZ,Xi,Xf,Zi,Zf)

%------
%INPUT
%------
%Pulso: Matriz de N sensores x M1 muestras con el pulso enviado.
%Eco: Matriz de N sensores x M2 muestras con el eco registrado
%pitch: Distancia entre sensores, en metros
%fs: Frecuencia de muestreo (en emisión y recepción, Hz)
%retardo: tiempo (s) que transcurre entre el comienzo del pulso y que se comienza a registrar el eco
%vel_prop: velocidad de propagación en el medio
%------
%------
%OPCIONALES
%------
%Zi
%Zf
%Xi
%Xf
%ResX: resolución de imagen deseada en X
%ResZ: resolución de imagen deseada en Z
%------

efs=fs;
rfs=fs;

matlabpool size;
poolFlag=0;
if (ans==0)
    matlabpool open
    poolFlag=1;
end
tic

c=vel_prop;
[M1,N]=size(pulso);
eco=[zeros(ceil(retardo*rfs),size(eco,2));eco];
[M2,N2]=size(eco);
if N2~=N
    disp('Warning: el pulso y el eco deben usar el mismo número de sensores')
    %Esto no es cierto
end
if efs~=rfs
    disp('Warning: El programa no está pensado para tener distinta frecuencia de muestreo en emisión y recepción')
    %Pero es arreglable
end
if nargin<8
    ResX=N;
    ResZ=1024;
end
if nargin<10
    Xi=0;
    Xf=pitch*(N-1);
end
if nargin<12
    Zi=retardo*c/2;
    Zf=Zi+M2*vel_prop/(2*rfs);
end

z1=retardo*c/2; %Debería ser dividido 2
minX=Xi;
maxX=Xf;
minZ=Zi;
maxZ=Zf;
if (minZ<z1)||(maxZ>(z1+M2*vel_prop/(2*rfs)))
    disp('Warning: usando profundidades (z) por fuera del rango de la señal')
end
DeltaZ=maxZ-minZ;
DeltaX=maxX-minX;
dZ=DeltaZ/ResZ;
dX=DeltaX/ResX;

if (dX<(c/rfs))
    disp('Warning: muestreo espacial por debajo del límite temporal (X)')
    %En el default, esto significa que los sensores están más separados de
    %lo que deberían!
end
if (dZ<(c/rfs))
    disp('Warning: muestreo espacial por debajo del límite temporal (Z)')
end

%Extiendo eco
zeros_end=ceil(rfs*sqrt(maxX^2+maxZ^2)/c); %Máximo delay en muestras
newM=M2+zeros_end;
newM=2^(ceil(log(newM)/log(2)));
eco_ext=zeros(newM,N2);
eco_ext(1:M2,:)=eco;
%Calculo transformada de Fourier
Feco_ext=fft(eco_ext);
Feco_ext=Feco_ext(1:(newM/2+1),:);
Feco=fft(eco);
Feco=Feco(1:(round(M2/2)+1),:);

%Extiendo pulso
newM1=M1+zeros_end;
newM1=2^(ceil(log(newM1)/log(2)));
pulso_ext=zeros(newM1,N);
pulso_ext(1:M1,:)=pulso;
%Calculo fft de pulso extendido
Fpulso_ext=fft(pulso_ext);
Fpulso_ext=Fpulso_ext(1:(newM1/2+1),:);
Fpulso=fft(pulso);
Fpulso=Fpulso(1:(M1/2+1),:);


imagen=zeros(ResX,ResZ);
parfor i=1:ResX
    i
    tic
    x=minX+(i-1)*dX;
    col_im=zeros(1,ResZ);
    for j=1:ResZ
        z=minZ + (j-1)* dZ;
        %Calculo respuesta generada por el pulso asumiendo sistema LIT
        pulso_llegada=propagacion(pulso_ext,x,z,c,pitch,efs,Fpulso_ext);
        %pulso_llegada=propagacion(pulso,x,z,c,pitch,efs,Fpulso);
        %Calculo imagen en el punto en cuestión
        col_im(j)=BeamFormA(eco_ext,pulso_llegada,x,z,c,pitch,rfs,z1,Feco_ext);
        %col_im(j)=BeamFormA(eco,pulso_llegada,x,z,c,pitch,rfs,z1,Feco);
    end
    imagen(i,:)=col_im;
    toc
end

if poolFlag==1
    matlabpool close
end
Z=minZ+[0:(ResZ-1)]*dZ;
X=minX+[0:(ResX-1)]*dX;
toc
end

function pix=BeamFormA(data,pulso,x,z,c,d,fs,~,Fdata)

%Idea: deconvoluciono la matriz de datos con el pulso en el origen (x,z) y
%además con la función de transferencia desde ese punto
%Trabajo en el espacio de Fourier por practicidad
%Trabajo solo con las frecuencias positivas, para ahorrar cuentas

[M,N]=size(data);
Fpulso=fft(pulso,M);

if nargin<9
    Fdata=fft(data); %FFT por columnas (por sensor)
    Fdata=Fdata(1:(M/2+1),:);
end


rx=[(0:(N-1))*d - x]';
modR=sqrt(rx.*rx + z^2); %Vector columna
delay=fs*(modR)'/c;
f= [0:(M/2-1),-M/2]'; %Vector columna
aux=2*pi*f*mod(delay/M,1);
Wb=cos(aux)+1i*sin(aux);
Wb(M/2+1,:)=2*cos(aux(end,:));
clear aux
YFpredata=Fdata.*Wb;
clear Wb
Pinverse=conj(Fpulso(1:(M/2+1)))./(abs(Fpulso(1:(M/2+1))).^2+max(abs(Fpulso(1:(M/2+1))))^2*10e-4);
YF=(YFpredata*modR).*Pinverse;
%YF=(YFpredata*modR).*conj(Fpulso(1:(M/2+1)))./(abs(Fpulso(1:(M/2+1)))+.0001);
clear YFpredata

%YF=(YFpredata*modR);
YF=[YF;conj(YF((M/2):-1:2))];
y=ifft(YF);
%[y,R]=deconv(y,pulso);



pix=y(1);

end

function pulso_en_llegada=propagacion(pulso,x,z,c,d,fs,Fpulso)

%Idea, propago el pulso en emisión hacia el punto (x,z) mediante la función
%de transferencia apropiada (asumiendo propagación estilo Huygens)
%Trabajo en espacio de Fourier
%Trabajo con las frecuencias positivas para ahorrar cuentas

[M,N]=size(pulso);
if nargin<7
    Fpulso=fft(pulso); %FFT por columnas (por sensor)
    Fpulso=Fpulso(1:(M/2+1),:);
end


rx=([0:(N-1)]*d - x)';
modR=sqrt(rx.*rx + z^2); %Vector columna
delay=fs*(modR)'/c;

f= [0:(M/2-1),-M/2]'; %Vector columna
aux=-2*pi*f*mod(delay/M,1);
Wb=cos(aux)+1i*sin(aux);
Wb(M/2+1,:)=cos(aux(end,:));
clear aux
YFpredata=Fpulso.*Wb;
clear Wb
YF=(YFpredata*(1./modR));
clear YFpredata
YF=[YF;conj(YF((M/2):-1:2))];

pulso_en_llegada=ifft(YF);

end